#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

// 订阅的无人机状态数据
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

// 订阅的无人机当前位置数据
geometry_msgs::PoseStamped local_pos;
void local_pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    local_pos = *msg;
}

// 订阅的期望位置数据
geometry_msgs::PoseStamped expect_pos;


geometry_msgs::Twist position_contrl_velocity(geometry_msgs::PoseStamped local_postion,geometry_msgs::PoseStamped expect_postion)
{
    geometry_msgs::Twist velocity_cmd; 
    velocity_cmd.linear.x = (expect_postion.pose.position.x - local_postion.pose.position.x);
    velocity_cmd.linear.y = (expect_postion.pose.position.y - local_postion.pose.position.y);
    velocity_cmd.linear.z = (expect_postion.pose.position.z - local_postion.pose.position.z);
    velocity_cmd.angular.x = 0.0;
    velocity_cmd.angular.y = 0.0;
    velocity_cmd.angular.z = 0.0;
    if(velocity_cmd.linear.x > 2.0)
    {
        velocity_cmd.linear.x = 2.0;
    }
    else if(velocity_cmd.linear.x < -2.0)
    {
        velocity_cmd.linear.x = -2.0;
    }
    if(velocity_cmd.linear.y > 2.0)
    {
        velocity_cmd.linear.y = 2.0;
    }
    else if(velocity_cmd.linear.y < -2.0)
    {
        velocity_cmd.linear.y = -2.0;
    }
    if(velocity_cmd.linear.z > 2.0)
    {
        velocity_cmd.linear.z = 2.0;
    }
    else if(velocity_cmd.linear.z < -2.0)
    {
        velocity_cmd.linear.z = -2.0;
    }
    
    return velocity_cmd;
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "offboard_cluster_iris0");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("iris_0/mavros/state", 10, state_cb);

    ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>
            ("iris_0/mavros/local_position/pose", 10, local_pos_cb);

    ros::Publisher local_velocity_pub = nh.advertise<geometry_msgs::Twist>
            ("iris_0/mavros/setpoint_velocity/cmd_vel_unstamped", 10);
    
    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    expect_pos.pose.position.x = 5;
    expect_pos.pose.position.y = 4;
    expect_pos.pose.position.z = 3;



    geometry_msgs::Twist velocity;
    velocity.linear.x = 0.0;
    velocity.linear.y = 0.0;
    velocity.linear.z = 0.0;
    velocity.angular.x = 0.0;
    velocity.angular.y = 0.0;  
    velocity.angular.z = 0.0;
    //send a few setpoints before starting
    for(int i = 100; ros::ok() && i > 0; --i){
        local_velocity_pub.publish(velocity);
        ros::spinOnce();
        rate.sleep();
    }


    float circle_radius = 10.0; // 设置圆的半径
    float circle_center_x = 0.0; // 设置圆心的x坐标
    float circle_center_y = 0.0; // 设置圆心的y坐标
    float theta = 0.0; // 角度

    // while(ros::ok() && !current_state.armed)
    // {
    //     local_velocity_pub.publish(velocity);
    //     ros::spinOnce();
    //     rate.sleep();
    // }

    while(ros::ok()){
  
        
        // 计算期望的圆轨迹
        expect_pos.pose.position.x = circle_center_x + circle_radius * cos(theta);
        expect_pos.pose.position.y = circle_center_y + circle_radius * sin(theta);
        expect_pos.pose.position.z = 3; // 设定期望的高度

        local_velocity_pub.publish(position_contrl_velocity(local_pos,expect_pos));
        ROS_INFO("local_postion: x[%.2f] y[%.2f] z[%.2f]",local_pos.pose.position.x,local_pos.pose.position.y,local_pos.pose.position.z);
        
        // 增加角度，每次增加0.2弧度
        theta += 0.02;

        // 限制角度在0到2π之间
        if (theta >= 2 * M_PI) {
            theta = 0;
        }

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

